#!/usr/bin/env python

import rospy, os
from task_planner.srv import *
from task_planner.msg import *
from roah_devices.srv import Bool
from roah_devices.msg import DevicesState
from std_srvs.srv import Empty

env = None
status_topic = None


############# CLASS ###################
class Environment_class:
	
	def __init__(self):
		# lightness
		self.p_LightON = False
		self.p_BlindsON = False
		self.p_Presence = True
		self.p_AutoMode = True
		self.p_SunLight = False
		self.p_LightButton = False
		self.p_BlindsButton = False
		################################		
		# help request
		self.home = True
		self.position1 = False
		self.position2 = False
		self.call1 = False
		self.call2 = False
		self.command1 = False
		self.command2 = False
	
	
	def update(self, p_LightON = None, p_BlindsON = None, p_Presence = None, p_AutoMode = None, p_SunLight = None,
				p_LightButton = None, p_BlindsButton = None, home = None, position1 = None, position2 = None,
				call1 = None, call2 = None, command1 = None, command2 = None):
		#lightness
		if p_LightON != None:
			self.p_LightON = p_LightON
		if p_BlindsON != None:
			self.p_BlindsON = p_BlindsON
		if p_Presence != None:
			self.p_Presence = p_Presence
		if p_AutoMode != None:
			self.p_AutoMode = p_AutoMode
		if p_SunLight != None:
			self.p_AutoMode = p_SunLight
		if p_LightButton != None:
			self.p_AutoMode = p_LightButton
		if p_BlindsButton != None:
			self.p_AutoMode = p_BlindsButton
		#help request
		if home != None:
			self.home = home
		if position1 != None:
			self.position1 = position1
		if position2 != None:
			self.position2 = position2
		if call1 != None:
			self.call1 = call1
		if call2 != None:
			self.call2 = call2
		if command1 != None:
			self.command1 = command1
		if command2 != None:
			self.command2 = command2
			
		publish_status()

################## SERVICE HANDLERS ################################
	
def handle_set_light(req):
	rospy.wait_for_service('/devices/switch_1/set')
	rospy.wait_for_service('/devices/switch_2/set')
	rospy.wait_for_service('/devices/switch_3/set')
	try:
		s1 = rospy.ServiceProxy('/devices/switch_1/set', Bool)
		s2 = rospy.ServiceProxy('/devices/switch_2/set', Bool)
		s3 = rospy.ServiceProxy('/devices/switch_3/set', Bool)
		s1(req.data)
		s2(req.data)
		s3(req.data)
		env.update(p_LightON = True)
	except rospy.ServiceException, e:
		print "error during setting the lights"
		
def handle_set_blinds(req):
	rospy.wait_for_service('/devices/blinds/max')
	rospy.wait_for_service('/devices/blinds/min')
	try:
		max = rospy.ServiceProxy('/devices/blinds/max', Empty)
		min = rospy.ServiceProxy('/devices/blinds/min', Empty)
		if req.data == True:
			max()
			env.update(p_BlindsON = True)
		else:
			min()
			env.update(p_BlindsON = False)
	except rospy.ServiceException, e:
		print "error during setting the p_BlindsON"

def handle_set_presence(req):
	env.update(p_Presence = req.data)
		
def handle_set_automatic_mode(req):
	env.update(p_AutoMode = req.data)
	
def handle_set_SunLight(req):
	env.update(p_SunLight = req.data)
	
def handle_set_LightButton(req):
	env.update(p_LightButton = req.data)
	
def handle_set_BlindsButton(req):
	env.update(p_BlindsButton = req.data)
	
def handle_set_home(req):
	env.update(home = req.data, position1 = not(req.data), position2 = not(req.data))
	
def handle_set_position1(req):
	env.update(home = not(req.data), position1 = req.data, position2 = not(req.data))
	
def handle_set_position2(req):
	env.update(home = not(req.data), position1 = not(req.data), position2 = req.data)
	
def handle_set_call1(req):
	env.update(call1 = req.data)
	
def handle_set_call2(req):
	env.update(call2 = req.data)
	
def handle_set_command1(req):
	env.update(command1 = req.data)
	
def handle_set_command2(req):
	env.update(command2 = req.data)
	
	
######################## MESSAGES ###################################

def server_status_callback(status):
	env.update(p_LightON = status.switch_1, p_BlindsON = status.blinds)
	
def ipcam_status_callback(status):
	print "ipcam_status_callback: TODO"
	
def magabot_position_status_callback(status):
	print "magabot_position_status_callback: TODO"
	
def publish_status():
	global status_topic
	print "- Publish on topic: environment/status"
	s = Environment()
	s.p_LightON = env.p_LightON
	s.p_BlindsON = env.p_BlindsON
	s.p_Presence = env.p_Presence
	s.p_AutoMode = env.p_AutoMode
	s.p_SunLight = env.p_SunLight
	s.p_LightButton = env.p_LightButton
	s.p_BlindsButton = env.p_BlindsButton
	status_topic.publish(s)	

########################## MAIN #####################################		

def start_interface():
	global status_topic
	
	os.system("clear") 
	print " Environment Interface running \n"
	
	rospy.init_node('environment_interface')
	
	#lightness
	rospy.Service('/environment/set_LightON', SetBool, handle_set_light)
	rospy.Service('/environment/set_BlindsON', SetBool, handle_set_blinds)
	rospy.Service('/environment/set_Presence', SetBool, handle_set_presence)
	rospy.Service('/environment/set_AutoMode', SetBool, handle_set_automatic_mode)
	rospy.Service('/environment/set_SunLight', SetBool, handle_set_SunLight)
	rospy.Service('/environment/set_LightButton', SetBool, handle_set_LightButton)
	rospy.Service('/environment/set_BlindsButton', SetBool, handle_set_BlindsButton)
	#help request
	rospy.Service('/environment/set_home', SetBool, handle_set_home)
	rospy.Service('/environment/set_position1', SetBool, handle_set_position1)
	rospy.Service('/environment/set_position2', SetBool, handle_set_position2)
	rospy.Service('/environment/set_call1', SetBool, handle_set_call1)
	rospy.Service('/environment/set_call2', SetBool, handle_set_call2)
	rospy.Service('/environment/set_command1', SetBool, handle_set_command1)
	rospy.Service('/environment/set_command2', SetBool, handle_set_command2)
	
	print " Premi un tasto per iniziare: "
	raw_input()

	#topics
	rospy.Subscriber("/devices/state", DevicesState, server_status_callback)
	#rospy.Subscriber("/ipcam/status", CamStatus, ipcam_status_callback)
	#rospy.Subscriber("/magabot/position", MagabotPosition, magabot_position_status_callback)
	status_topic = rospy.Publisher('/environment/status', Environment)
	
	print " Premi un tasto..."
	raw_input()
	
	publish_status()
	
	rospy.spin()
	
if __name__ == '__main__':
	env = Environment_class()
	start_interface()
